/*
 * slam_karto
 * Copyright (c) 2008, Willow Garage, Inc.
 *
 * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
 * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
 * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
 * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
 *
 * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
 * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
 * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
 * CONDITIONS.
 *
 */

/* Author: Brian Gerkey */

/**

@mainpage karto_gmapping

@htmlinclude manifest.html

*/
#include "std_srvs/Empty.h"
#include "ros/ros.h"
#include "ros/console.h"
#include "message_filters/subscriber.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "visualization_msgs/MarkerArray.h"

#include "nav_msgs/MapMetaData.h"
#include "sensor_msgs/LaserScan.h"
#include "nav_msgs/GetMap.h"

#include "open_karto/Mapper.h"

#include "spa_solver.h"

#include <boost/thread.hpp>

#include <string>
#include <map>
#include <vector>


#include <fstream>

ofstream  debugPrint_("/home/yhzhao/rosharp/src/rosharp/mapping/slam_karto/build/debugPrint_.txt"); //地址相关.

#define debug_flag_ true

// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))

class SlamKarto
{
public:
    SlamKarto();
    ~SlamKarto();

    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
    bool mapCallback(nav_msgs::GetMap::Request  &req,
                     nav_msgs::GetMap::Response &res);

private:
    bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t);
    karto::LaserRangeFinder* getLaser(const sensor_msgs::LaserScan::ConstPtr& scan);
    bool addScan(karto::LaserRangeFinder* laser,
                 const sensor_msgs::LaserScan::ConstPtr& scan,
                 karto::Pose2& karto_pose);
    bool updateMap();
    void publishTransform();
    void publishLoop(double transform_publish_period);
    void publishGraphVisualization();

    // ROS handles
    ros::NodeHandle node_;
    tf::TransformListener tf_;
    tf::TransformBroadcaster* tfB_;
    message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
    tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
    ros::Publisher sst_;
    ros::Publisher marker_publisher_;
    ros::Publisher sstm_;
    ros::ServiceServer ss_;
    ros::Publisher link_node_publisher_;

    // The map that will be published / send to service callers
    nav_msgs::GetMap::Response map_;

    // Storage for ROS parameters
    std::string odom_frame_;
    std::string map_frame_;
    std::string base_frame_;
    int throttle_scans_;
    ros::Duration map_update_interval_;
    double resolution_;
    boost::mutex map_mutex_;
    boost::mutex map_to_odom_mutex_;

    //message debug
    bool debug_print_flag_;

    // Karto bookkeeping
    karto::Mapper* mapper_;
    karto::Dataset* dataset_;
    SpaSolver* solver_;
    std::map<std::string, karto::LaserRangeFinder*> lasers_;
    std::map<std::string, bool> lasers_inverted_;

    // Internal state
    bool got_map_;
    int laser_count_;
    boost::thread* transform_thread_;
    tf::Transform map_to_odom_;
    unsigned marker_count_;
    bool inverted_laser_;
};

SlamKarto::SlamKarto() :
    got_map_(false),
    laser_count_(0),
    transform_thread_(NULL),
    marker_count_(0)
{
    map_to_odom_.setIdentity();//********

    // Retrieve parameters
    ros::NodeHandle private_nh_("~");
    if(!private_nh_.getParam("debug_print_flag", debug_print_flag_))
        debug_print_flag_ = false;
    if(debug_print_flag_)
        debugPrint_<<"0.0:  start record the message!"<<endl;


    if(!private_nh_.getParam("odom_frame", odom_frame_))
        odom_frame_ = "odom";
    if(!private_nh_.getParam("map_frame", map_frame_))
        map_frame_ = "map";
    if(!private_nh_.getParam("base_frame", base_frame_))
        base_frame_ = "base_link";
    if(!private_nh_.getParam("throttle_scans", throttle_scans_))
        throttle_scans_ = 1;
    double tmp;
    if(!private_nh_.getParam("map_update_interval", tmp))
        tmp = 25;//tmp = 5.0;
    map_update_interval_.fromSec(tmp);
    if(!private_nh_.getParam("resolution", resolution_))
    {
        // Compatibility with slam_gmapping, which uses "delta" to mean
        // resolution
        if(!private_nh_.getParam("delta", resolution_))
            resolution_ = 0.025;// resolution_ = 0.05;
    }
    double transform_publish_period;
    private_nh_.param("transform_publish_period", transform_publish_period, 0.05);

    if(debug_print_flag_)
        debugPrint_<<"init the param for slam"<<endl;

    // Set up advertisements and subscriptions
    tfB_ = new tf::TransformBroadcaster();
    sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
    sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
    ss_ = node_.advertiseService("dynamic_map", &SlamKarto::mapCallback, this);
    scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
    scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
    scan_filter_->registerCallback(boost::bind(&SlamKarto::laserCallback, this, _1));
    marker_publisher_ = node_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array",1);

    link_node_publisher_ = node_.advertise<geometry_msgs::Point>("link_node_",10);

    // Create a thread to periodically publish the latest map->odom
    // transform; it needs to go out regularly, uninterrupted by potentially
    // long periods of computation in our main loop.
    transform_thread_ = new boost::thread(boost::bind(&SlamKarto::publishLoop, this, transform_publish_period));

    // Initialize Karto structures
    mapper_ = new karto::Mapper();
    dataset_ = new karto::Dataset();

    if(1) //参数设置
    {
        // Setting General Parameters from the Parameter Server
        bool use_scan_matching;
        if(private_nh_.getParam("use_scan_matching", use_scan_matching))
            mapper_->setParamUseScanMatching(use_scan_matching);

        bool use_scan_barycenter;
        if(private_nh_.getParam("use_scan_barycenter", use_scan_barycenter))
            mapper_->setParamUseScanBarycenter(use_scan_barycenter);

        double minimum_travel_distance;
        if(private_nh_.getParam("minimum_travel_distance", minimum_travel_distance))
            mapper_->setParamMinimumTravelDistance(minimum_travel_distance);

        double minimum_travel_heading;
        if(private_nh_.getParam("minimum_travel_heading", minimum_travel_heading))
            mapper_->setParamMinimumTravelHeading(minimum_travel_heading);

        int scan_buffer_size;
        if(private_nh_.getParam("scan_buffer_size", scan_buffer_size))
            mapper_->setParamScanBufferSize(scan_buffer_size);

        double scan_buffer_maximum_scan_distance;
        if(private_nh_.getParam("scan_buffer_maximum_scan_distance", scan_buffer_maximum_scan_distance))
            mapper_->setParamScanBufferMaximumScanDistance(scan_buffer_maximum_scan_distance);

        double link_match_minimum_response_fine;
        if(private_nh_.getParam("link_match_minimum_response_fine", link_match_minimum_response_fine))
            mapper_->setParamLinkMatchMinimumResponseFine(link_match_minimum_response_fine);

        double link_scan_maximum_distance;
        if(private_nh_.getParam("link_scan_maximum_distance", link_scan_maximum_distance))
            mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance);

        double loop_search_maximum_distance;
        if(private_nh_.getParam("loop_search_maximum_distance", loop_search_maximum_distance))
            mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance);

        bool do_loop_closing;
        if(private_nh_.getParam("do_loop_closing", do_loop_closing))
            mapper_->setParamDoLoopClosing(do_loop_closing);

        int loop_match_minimum_chain_size;
        if(private_nh_.getParam("loop_match_minimum_chain_size", loop_match_minimum_chain_size))
            mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size);

        double loop_match_maximum_variance_coarse;
        if(private_nh_.getParam("loop_match_maximum_variance_coarse", loop_match_maximum_variance_coarse))
            mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse);

        double loop_match_minimum_response_coarse;
        if(private_nh_.getParam("loop_match_minimum_response_coarse", loop_match_minimum_response_coarse))
            mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse);

        double loop_match_minimum_response_fine;
        if(private_nh_.getParam("loop_match_minimum_response_fine", loop_match_minimum_response_fine))
            mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine);

        // Setting Correlation Parameters from the Parameter Server

        double correlation_search_space_dimension;
        if(private_nh_.getParam("correlation_search_space_dimension", correlation_search_space_dimension))
            mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension);

        double correlation_search_space_resolution;
        if(private_nh_.getParam("correlation_search_space_resolution", correlation_search_space_resolution))
            mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution);

        double correlation_search_space_smear_deviation;
        if(private_nh_.getParam("correlation_search_space_smear_deviation", correlation_search_space_smear_deviation))
            mapper_->setParamCorrelationSearchSpaceSmearDeviation(correlation_search_space_smear_deviation);

        // Setting Correlation Parameters, Loop Closure Parameters from the Parameter Server

        double loop_search_space_dimension;
        if(private_nh_.getParam("loop_search_space_dimension", loop_search_space_dimension))
            mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension);

        double loop_search_space_resolution;
        if(private_nh_.getParam("loop_search_space_resolution", loop_search_space_resolution))
            mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution);

        double loop_search_space_smear_deviation;
        if(private_nh_.getParam("loop_search_space_smear_deviation", loop_search_space_smear_deviation))
            mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation);

        // Setting Scan Matcher Parameters from the Parameter Server

        double distance_variance_penalty;
        if(private_nh_.getParam("distance_variance_penalty", distance_variance_penalty))
            mapper_->setParamDistanceVariancePenalty(distance_variance_penalty);

        double angle_variance_penalty;
        if(private_nh_.getParam("angle_variance_penalty", angle_variance_penalty))
            mapper_->setParamAngleVariancePenalty(angle_variance_penalty);

        double fine_search_angle_offset;
        if(private_nh_.getParam("fine_search_angle_offset", fine_search_angle_offset))
            mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset);

        double coarse_search_angle_offset;
        if(private_nh_.getParam("coarse_search_angle_offset", coarse_search_angle_offset))
            mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset);

        double coarse_angle_resolution;
        if(private_nh_.getParam("coarse_angle_resolution", coarse_angle_resolution))
            mapper_->setParamCoarseAngleResolution(coarse_angle_resolution);

        double minimum_angle_penalty;
        if(private_nh_.getParam("minimum_angle_penalty", minimum_angle_penalty))
            mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty);

        double minimum_distance_penalty;
        if(private_nh_.getParam("minimum_distance_penalty", minimum_distance_penalty))
            mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty);

        bool use_response_expansion;
        if(private_nh_.getParam("use_response_expansion", use_response_expansion))
            mapper_->setParamUseResponseExpansion(use_response_expansion);
    }
    // Set solver to be used in loop closure
    solver_ = new SpaSolver();
    mapper_->SetScanSolver(solver_);
}

SlamKarto::~SlamKarto()
{
    if(transform_thread_)
    {
        transform_thread_->join();
        delete transform_thread_;
    }
    if (scan_filter_)
        delete scan_filter_;
    if (scan_filter_sub_)
        delete scan_filter_sub_;
    if (solver_)
        delete solver_;
    if (mapper_)
        delete mapper_;
    if (dataset_)
        delete dataset_;
    // TODO: delete the pointers in the lasers_ map; not sure whether or not
    // I'm supposed to do that.
}

void
SlamKarto::publishLoop(double transform_publish_period)
{
    if(transform_publish_period == 0)
        return;

    ros::Rate r(1.0 / transform_publish_period);
    while(ros::ok())
    {
        publishTransform();
        r.sleep();
    }
}

void
SlamKarto::publishTransform()
{
    boost::mutex::scoped_lock(map_to_odom_mutex_);
    ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.05);
    tfB_->sendTransform(tf::StampedTransform (map_to_odom_, ros::Time::now(), map_frame_, odom_frame_));
}

/**
 * @brief SlamKarto::getLaser
 * 将ros的scan数据转化到karto识别的数据.  多设备的laser数据   LaserRangeFinder数据中包含pose信息.  pose是scan的tf相对base的变换..认为是里程数据.
 * 判是否反转激光　　　添加laser rangefinder到 dataset中
 * lasers_ 主要是涉及多laser相关的设置..(frame_id不一样)
 * map使用的方法:
 * lasers_[scan->header.frame_id] = laser;
 *
 * @param scan
 * @return
 */
karto::LaserRangeFinder*
SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    // Check whether we know about this laser yet
    if(lasers_.find(scan->header.frame_id) == lasers_.end()) // not find
    {
        // New laser; need to create a Karto device for it.

        // Get the laser's pose, relative to base.
        tf::Stamped<tf::Pose> ident;
        tf::Stamped<tf::Transform> laser_pose;
        ident.setIdentity();
        ident.frame_id_ = scan->header.frame_id;
        ident.stamp_ = scan->header.stamp;
        try
        {
            tf_.transformPose(base_frame_, ident, laser_pose);//  scan的tf相对base的pose  laser_pose  ref to b
        }
        catch(tf::TransformException e)
        {
            ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
                     e.what());
            return NULL;
        }

        double yaw = tf::getYaw(laser_pose.getRotation());

        ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f",
                 scan->header.frame_id.c_str(),
                 laser_pose.getOrigin().x(),
                 laser_pose.getOrigin().y(),
                 yaw);
        // To account for lasers that are mounted upside-down, we determine the
        // min, max, and increment angles of the laser in the base frame.
        tf::Quaternion q;
        q.setRPY(0.0, 0.0, scan->angle_min);
        tf::Stamped<tf::Quaternion> min_q(q, scan->header.stamp,    //min_q  ref laser
                                          scan->header.frame_id);
        q.setRPY(0.0, 0.0, scan->angle_max);
        tf::Stamped<tf::Quaternion> max_q(q, scan->header.stamp,
                                          scan->header.frame_id);
        try
        {
            tf_.transformQuaternion(base_frame_, min_q, min_q); //  min_q  ref  base
            tf_.transformQuaternion(base_frame_, max_q, max_q);
        }
        catch(tf::TransformException& e)
        {
            ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
                     e.what());
            return NULL;
        }

        double angle_min = tf::getYaw(min_q);
        double angle_max = tf::getYaw(max_q);
        bool inverse =  lasers_inverted_[scan->header.frame_id] = angle_max < angle_min;  //　　inverse?
        if (inverse)
            ROS_INFO("laser is mounted upside-down");


        // Create a laser range finder device and copy in data from the first
        // scan
        std::string name = scan->header.frame_id;
        karto::LaserRangeFinder* laser =
                karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));

        laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
                                          laser_pose.getOrigin().y(),
                                          yaw));
        laser->SetMinimumRange(scan->range_min);
        laser->SetMaximumRange(scan->range_max);
        laser->SetMinimumAngle(scan->angle_min);
        laser->SetMaximumAngle(scan->angle_max);
        laser->SetAngularResolution(scan->angle_increment);
        // TODO: expose this, and many other parameters
        //laser_->SetRangeThreshold(12.0);

        // Store this laser device for later
        lasers_[scan->header.frame_id] = laser; // current laser


        if(debug_flag_)
            debugPrint_<<laser_pose.getOrigin().x()<<" "<<laser_pose.getOrigin().y()<<" "<<yaw<<" "<<scan->range_min
                      <<" "<<scan->range_max<<" " <<scan->angle_min<<" "<<scan->angle_max<<" "<<scan->angle_increment<<endl;



        // Add it to the dataset, which seems to be necessary
        dataset_->Add(laser);
    }

    return lasers_[scan->header.frame_id];
}

bool
SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
{
    // Get the robot's pose
    tf::Stamped<tf::Pose> ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0),
                                               tf::Vector3(0,0,0)), t, base_frame_);
    tf::Stamped<tf::Transform> odom_pose;
    try
    {
        tf_.transformPose(odom_frame_, ident, odom_pose);
    }
    catch(tf::TransformException e)
    {
        ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
        return false;
    }
    double yaw = tf::getYaw(odom_pose.getRotation());

    karto_pose = karto::Pose2(odom_pose.getOrigin().x(),
                         odom_pose.getOrigin().y(),
                         yaw);
    return true;
}

void
SlamKarto::publishGraphVisualization()
{
    geometry_msgs::Point  link;

    std::vector<float> graph;
    solver_->getGraph(graph);

    visualization_msgs::MarkerArray marray;

    visualization_msgs::Marker m;
    m.header.frame_id = "map";
    m.header.stamp = ros::Time::now();
    m.id = 0;
    m.ns = "karto";
    m.type = visualization_msgs::Marker::SPHERE;
    m.pose.position.x = 0.0;
    m.pose.position.y = 0.0;
    m.pose.position.z = 0.0;
    m.scale.x = 0.1;
    m.scale.y = 0.1;
    m.scale.z = 0.1;
    m.color.r = 1.0;
    m.color.g = 0;
    m.color.b = 0.0;
    m.color.a = 1.0;
    m.lifetime = ros::Duration(0);

    visualization_msgs::Marker edge;
    edge.header.frame_id = "map";
    edge.header.stamp = ros::Time::now();
    edge.action = visualization_msgs::Marker::ADD;
    edge.ns = "karto";
    edge.id = 0;
    edge.type = visualization_msgs::Marker::LINE_STRIP;
    edge.scale.x = 0.01;
//    edge.scale.y = 0.1;
//    edge.scale.z = 0.1;
    edge.color.a = 1.0;
    edge.color.r = 0.0;
    edge.color.g = 0.0;
    edge.color.b = 1.0;

    m.action = visualization_msgs::Marker::ADD;
    uint id = 0;
    for (uint i=0; i<graph.size()/2; i++)
    {
        if(id ==0)
        {
            m.color.r = 1.0;
            m.color.g = 1.0;
            m.color.b = 0.0;
        }

        if(id == graph.size()/2 -1)
        {
            m.color.r = 0.5;
            m.color.g = 0.5;
            m.color.b = 0.0;
        }
        if(id%10 == 1) //
        {
            m.color.r = 0.0;
            m.color.g = 1.0;
            m.color.b = 0.0;
        }
        else
        {
            m.color.r = 1.0;
            m.color.g = 0;
            m.color.b = 0.0;
        }

        m.id = id;
        m.pose.position.x = graph[2*i];
        m.pose.position.y = graph[2*i+1];
        marray.markers.push_back(visualization_msgs::Marker(m));
        id++;

        if(i>0)
        {
            edge.points.clear();

            geometry_msgs::Point p;
            p.x = graph[2*(i-1)];
            p.y = graph[2*(i-1)+1];
            edge.points.push_back(p);
            p.x = graph[2*i];
            p.y = graph[2*i+1];
            edge.points.push_back(p);
            edge.id = id;

            marray.markers.push_back(visualization_msgs::Marker(edge));
            id++;
        }
    }

    m.action = visualization_msgs::Marker::DELETE;
    for (; id < marker_count_; id++)
    {
        m.id = id;
        marray.markers.push_back(visualization_msgs::Marker(m));
    }

    marker_count_ = marray.markers.size();

    marker_publisher_.publish(marray);

}



void
SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    laser_count_++;

    if ((laser_count_ % throttle_scans_) != 0)  //1      激光频率问题
        return;

    static ros::Time last_map_update(0,0);

    // Check whether we know about this laser yet // if New laser, need to create a Karto device for it.
    karto::LaserRangeFinder* laser = getLaser(scan);

    if(!laser)
    {
        ROS_WARN("Failed to create laser device for %s; discarding scan",
                 scan->header.frame_id.c_str());
        return;
    }
    if(debug_print_flag_)
        debugPrint_<<"1.0 laser_count_: "<<laser_count_<<"   addScan  "<<"  "<<endl;

    karto::Pose2 odom_pose;
    if(addScan(laser, scan, odom_pose))
    {
        ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
                  odom_pose.GetX(),
                  odom_pose.GetY(),
                  odom_pose.GetHeading());

        ROS_INFO("added scan at pose: %.3f %.3f %.3f",
                 odom_pose.GetX(),
                 odom_pose.GetY(),
                 odom_pose.GetHeading());

        geometry_msgs::Point debug_pose;
        debug_pose.x = odom_pose.GetX();
        debug_pose.y = odom_pose.GetY();
        debug_pose.z = odom_pose.GetHeading();

        link_node_publisher_.publish(debug_pose);
        if(debug_print_flag_)
            debugPrint_<<"2.0  odom_pose: "<<odom_pose.GetX()<<" "<<odom_pose.GetY()<<"  "<<odom_pose.GetHeading()<<endl;

        publishGraphVisualization();

        if(!got_map_ ||
                (scan->header.stamp - last_map_update) > map_update_interval_)
        {
            if(updateMap())
            {
                last_map_update = scan->header.stamp;
                got_map_ = true;
                ROS_DEBUG("Updated the map");
                if(debug_print_flag_)
                    debugPrint_<<"3.0  updateMap: "<<endl;
            }
        }
    }
}

bool
SlamKarto::updateMap()
{
    boost::mutex::scoped_lock(map_mutex_);

    karto::OccupancyGrid* occ_grid =
            karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);//occ_grid

    if(!occ_grid)
        return false;

    if(!got_map_) {
        map_.map.info.resolution = resolution_;
        map_.map.info.origin.position.x = 0.0;
        map_.map.info.origin.position.y = 0.0;
        map_.map.info.origin.position.z = 0.0;
        map_.map.info.origin.orientation.x = 0.0;
        map_.map.info.origin.orientation.y = 0.0;
        map_.map.info.origin.orientation.z = 0.0;
        map_.map.info.origin.orientation.w = 1.0;
    }

    // Translate to ROS format
    kt_int32s width = occ_grid->GetWidth();
    kt_int32s height = occ_grid->GetHeight();
    karto::Vector2<kt_double> offset = occ_grid->GetCoordinateConverter()->GetOffset();

    if(map_.map.info.width != (unsigned int) width ||
            map_.map.info.height != (unsigned int) height ||
            map_.map.info.origin.position.x != offset.GetX() ||
            map_.map.info.origin.position.y != offset.GetY())
    {
        map_.map.info.origin.position.x = offset.GetX();
        map_.map.info.origin.position.y = offset.GetY();
        map_.map.info.width = width;
        map_.map.info.height = height;
        map_.map.data.resize(map_.map.info.width * map_.map.info.height);
    }

    for (kt_int32s y=0; y<height; y++)
    {
        for (kt_int32s x=0; x<width; x++)
        {
            // Getting the value at position x,y
            kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));

            switch (value)
            {
            case karto::GridStates_Unknown:
                map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
                break;
            case karto::GridStates_Occupied:
                map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
                break;
            case karto::GridStates_Free:
                map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
                break;
            default:
                ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
                break;
            }
        }
    }

    // Set the header information on the map
    map_.map.header.stamp = ros::Time::now();
    map_.map.header.frame_id = map_frame_;

    sst_.publish(map_.map);
    sstm_.publish(map_.map.info);

    delete occ_grid;

    return true;
}

/**
 * @brief SlamKarto::addScan
 * @param laser　       　LaserRangeFinder　ref base
 * @param scan           传感器数据　　　　　ref  laser
 * @param karto_pose     odom_pose       回调
 * @return
 */
bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
                   const sensor_msgs::LaserScan::ConstPtr& scan,
                   karto::Pose2& karto_pose)
{
    if(!getOdomPose(karto_pose, scan->header.stamp)) //　  将odom与base保持坐标系对齐．　返回 odom  --> karto_pose
        return false;

    // Create a vector of doubles for karto
    std::vector<kt_double> readings;
    //readings       原始scan laser -> reading
    if (lasers_inverted_[scan->header.frame_id])
    {
        for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
            it != scan->ranges.rend();
            ++it)
        {
            readings.push_back(*it);  //laser range messages
        }
    }
    else
    {
        for(std::vector<float>::const_iterator it = scan->ranges.begin();
            it != scan->ranges.end();
            ++it)
        {
            readings.push_back(*it);
            debugPrint_<<"   "<< *it;
        }

    }
    debugPrint_<<"   " <<endl;
    if(debug_flag_)
        debugPrint_<<"reading.size()  "<<readings.size()<<endl;

    // create localized range scan   创建rangeScan
    karto::LocalizedRangeScan* range_scan =
            new karto::LocalizedRangeScan(laser->GetName(), readings);//

    range_scan->SetOdometricPose(karto_pose);// Sets the odometric pose of this scan
    range_scan->SetCorrectedPose(karto_pose);// Moves the scan by moving the robot pose to the given location.

    if(debug_print_flag_)
        debugPrint_<<"1.1 addScan-karto_pose-: "<<karto_pose.GetX()<<"   "<<karto_pose.GetY()<<"  "<< karto_pose.GetHeading() <<"  "<<endl;

    // Add the localized range scan to the mapper
    bool processed;          //range_scan  contain reading and predict pose of robot
    if((processed = mapper_->Process(range_scan))) // pose 为base相对odom的pose
    {
        //std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
        karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
        if(debug_print_flag_)
            debugPrint_<<"1.2 addScan-corrected_pose-: "<<corrected_pose.GetX()<<"   "<<corrected_pose.GetY()<<"  "<< corrected_pose.GetHeading() <<"  "<<endl;

        // Compute the map->odom transform
        tf::Stamped<tf::Pose> odom_to_map;
        try
        {
            tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform( tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
                                                                                tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
                                                                 scan->header.stamp, base_frame_),odom_to_map);
        }
        catch(tf::TransformException e)
        {
            ROS_ERROR("Transform from base_link to odom failed\n");
            odom_to_map.setIdentity();
        }

        map_to_odom_mutex_.lock();
        map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
                                     tf::Point(      odom_to_map.getOrigin() ) ).inverse();
        map_to_odom_mutex_.unlock();

        //debugPrint_<< " x y z "<< odom_to_map.getOrigin().getX() <<"   "<< odom_to_map.getOrigin().getY()<<"  "<<odom_to_map.getOrigin().getZ()<<endl;
        if(debug_print_flag_)
            debugPrint_<<"1.3 addScan-odom_to_map: "<<odom_to_map.getOrigin().getX() <<"   "<< odom_to_map.getOrigin().getY()<<"  "<<endl;
        // Add the localized range scan to the dataset (for memory management)
        dataset_->Add(range_scan);
    }
    else
        delete range_scan;

    return processed;
}

bool 
SlamKarto::mapCallback(nav_msgs::GetMap::Request  &req,
                       nav_msgs::GetMap::Response &res)
{
    boost::mutex::scoped_lock(map_mutex_);
    if(got_map_ && map_.map.info.width && map_.map.info.height)
    {
        res = map_;
        return true;
    }
    else
        return false;
}

int
main(int argc, char** argv)
{
    ros::init(argc, argv, "slam_karto");

    SlamKarto kn;

    ros::spin();

    return 0;
}
